#include "seg_matrix.h"
#include "math.h"

static int is[S_PIX_NUM];
static int js[S_PIX_NUM];
static float g_p_transition_matrix[S_PIX_NUM * S_PIX_NUM];
static float g_p_fundamental_matrix[S_PIX_NUM * S_PIX_NUM];
static float g_p_absorbed_time_vector[S_PIX_NUM * S_PIX_NUM];
static S_Pix* g_p_fg_spix[S_PIX_NUM];
static S_Pix* g_p_bg_spix[S_PIX_NUM];
static int matrix_inv(float a[], int n);

void create_fundamental_matrix(unsigned char* p_src_img, int img_width, int img_height, sp_output* p_sp, int touch_x, int touch_y, int fg_num)
{
	short* p_s_pix_map  = p_sp->p_s_pix_map;
    int valid_s_pix_num = p_sp->valid_s_pix_num;
    S_Pix* p_spix       = p_sp->p_spix;
    
	for (int m = 0; m < valid_s_pix_num * valid_s_pix_num; m++)
    {
        g_p_fundamental_matrix[m] = 0.0f;
    }

	for (int m = 0; m < valid_s_pix_num; m++)
    {
        g_p_absorbed_time_vector[m] = -999999.0f;
    }

    for (int m = 0; m < fg_num; m++)
    {
		for (int n = 0; n < fg_num; n++)
		{
			g_p_fundamental_matrix[m * fg_num + n] = g_p_transition_matrix[m * valid_s_pix_num + n];
		}
    }

	int inv_is_ok = matrix_inv(g_p_fundamental_matrix, fg_num);

	if (inv_is_ok)
	{
		float min_fundamental_value = 999999.0f;
		for (int m = 0; m < fg_num; m++)
		{
			float sum = 0.0f;
			for (int n = 0; n < fg_num; n++)
			{
				sum += g_p_fundamental_matrix[m * fg_num + n];
			}
			g_p_absorbed_time_vector[m] = sum;
			if (min_fundamental_value > g_p_absorbed_time_vector[m])
			{
				min_fundamental_value = g_p_absorbed_time_vector[m];
			}
		}

		float min_value =  99999.0f;
		float max_value = -99999.0f;

		for (int m = 0; m < fg_num; m++)
		{
			g_p_absorbed_time_vector[m] -= min_fundamental_value;
			if (g_p_absorbed_time_vector[m] <= 0)
			{
				continue;
			}
			if (min_value > g_p_absorbed_time_vector[m])
			{
				min_value = g_p_absorbed_time_vector[m];
			}
			if (max_value < g_p_absorbed_time_vector[m])
			{
				max_value = g_p_absorbed_time_vector[m];
			}
		}

		float max_min = max_value - min_value;
		if (max_min > 0)
		{
			for (int m = 0; m < fg_num; m++)
			{
				if (g_p_absorbed_time_vector[m] <= 0)
				{
					p_spix[m].salient_conf = 0;
					continue;
				}
				g_p_absorbed_time_vector[m] =  255.0f * (g_p_absorbed_time_vector[m] - min_value) / max_min;
				int ori_id = g_p_fg_spix[m]->merge_id;
				p_spix[ori_id].salient_conf = g_p_absorbed_time_vector[m];
			}
		}
	}
	int sun = 0;
}




int create_affinity_matrix(unsigned char* p_src_img, int img_width, int img_height, sp_output* p_sp, int touch_x, int touch_y)
{
    short* p_s_pix_map  = p_sp->p_s_pix_map;
    int valid_s_pix_num = p_sp->valid_s_pix_num;
    S_Pix* p_spix       = p_sp->p_spix;

    int bg_count = 0, fg_count = 0;
  
    for (int i = 0; i < valid_s_pix_num; ++i)
    {
        p_spix[i].merge_id = i;
		if (p_spix[i].is_bg)
        {
            g_p_bg_spix[bg_count] = &p_spix[i]; 
            bg_count++;
        }
        else
        {
            g_p_fg_spix[fg_count] = &p_spix[i];          
            fg_count++;            
        }
        p_sp->p_spix[i].merge_id = i;
    }
    
    for (int m = 0; m < valid_s_pix_num * valid_s_pix_num; m++)
    {
        g_p_transition_matrix[m] = 0.0f;
    }

    for (int j = 0, i = 0; i < valid_s_pix_num; ++j, ++i)
    {
		g_p_transition_matrix[i * valid_s_pix_num + j] = 1.0f;
	}

    for (int i = 0; i < fg_count; ++i)
    {
		float sum = 0;
        for (int j = 0; j < fg_count; ++j)
        {
			if (i != j)
			{
				int channel_0_dis  = (g_p_fg_spix[i]->color.c0 - g_p_fg_spix[j]->color.c0) * (g_p_fg_spix[i]->color.c0 - g_p_fg_spix[j]->color.c0);
				int channel_1_dis  = (g_p_fg_spix[i]->color.c1 - g_p_fg_spix[j]->color.c1) * (g_p_fg_spix[i]->color.c1 - g_p_fg_spix[j]->color.c1);
				int channel_2_dis  = (g_p_fg_spix[i]->color.c2 - g_p_fg_spix[j]->color.c2) * (g_p_fg_spix[i]->color.c2 - g_p_fg_spix[j]->color.c2);
				int all_channel_dis   = channel_0_dis * COLOR_MODIFY_RATE + channel_1_dis + channel_2_dis; 
				float affinity_weight = expf(-(float)all_channel_dis / 5000.0f);
				g_p_transition_matrix[i * valid_s_pix_num + j] = affinity_weight;
				sum += affinity_weight;
			}
			else
			{
				sum += 1.0f;
			}
        }
		for (int j = 0; j < fg_count; ++j)
        {
			if (sum > 0)
			{
				g_p_transition_matrix[i * valid_s_pix_num + j] = g_p_transition_matrix[i * valid_s_pix_num + j] / sum;
			}
			else
			{
				g_p_transition_matrix[i * valid_s_pix_num + j] = 0.0f;
			}
		}
    }

	return fg_count;
}


int matrix_inv(float a[], int n)  
{ 
    int i,j,k,l,u,v;  
    float d,p;  

    for (k=0; k<=n-1; k++)  
    {  
        d=0.0; 
        for (i=k; i<=n-1; i++)  
        for (j=k; j<=n-1; j++)  
        {  
            l=i*n+j; p=fabs(a[l]);  
            if (p>d) { d=p; is[k]=i; js[k]=j;}  
        }  
        if (d+1.0==1.0)  
        {  
            return(0);  
        }  
        if (is[k]!=k)  
            for (j=0; j<=n-1; j++)  
            {  
                u=k*n+j; v=is[k]*n+j;  
                p=a[u]; a[u]=a[v]; a[v]=p;  
            }  
        if (js[k]!=k)  
            for (i=0; i<=n-1; i++)  
            {  
                u=i*n+k; v=i*n+js[k];  
                p=a[u]; a[u]=a[v]; a[v]=p;  
            }  
        l=k*n+k;  
        a[l]=1.0/a[l];  
        for (j=0; j<=n-1; j++)  
            if (j!=k)  
            { u=k*n+j; a[u]=a[u]*a[l];}  
        for (i=0; i<=n-1; i++)  
            if (i!=k)  
                for (j=0; j<=n-1; j++)  
        if (j!=k)  
        {  
            u=i*n+j;  
            a[u] -= a[i*n+k]*a[k*n+j];  
        }  
        for (i=0; i<=n-1; i++)  
            if (i!=k)  
            {  
                u=i*n+k;  
                a[u] = -a[u]*a[l];  
            }  
    }  
    for (k=n-1; k>=0; k--)  
    {  
        if (js[k]!=k)  
        for (j=0; j<=n-1; j++)  
        {  
            u=k*n+j; v=js[k]*n+j;  
        p=a[u]; a[u]=a[v]; a[v]=p;  
        }  
        if (is[k]!=k)  
        for (i=0; i<=n-1; i++)  
        {   
            u=i*n+k; v=i*n+is[k];  
            p=a[u]; a[u]=a[v]; a[v]=p;  
        }  
    }  
    return(1);  
}  
  
// C = A*B  
void matrix_multiply(float a[], float b[],int m, int n, int k, float c[])  
{  
    int i,j,l,u;  
    for (i=0; i<=m-1; i++)  
        for (j=0; j<=k-1; j++)  
        {  
            u=i*k+j; c[u]=0.0;  
            for (l=0; l<=n-1; l++)  
                c[u] += a[i*n+l]*b[l*k+j]; 
        }  
}